/*
 * findTags.cpp
 *
 *  Created on: Nov 4, 2012
 *      Author: Matthijs
 */
#include <camera/camera.h>

using namespace cv;

/**
 *  Find tags in the currently loaded image frames.
 */
void Camera::findTags (Mat src)
{
	if (!src.cols)
	{
		ROS_ERROR("Invalid haystack provided.");
		return;
	}

	tag haystack;
	haystack.image = src;

	//-- Step 1: Detect the keypoints using SURF Detector
	detector.detect( src, haystack.keypoints );

	//-- Step 2: Calculate descriptors (feature vectors)
	extractor.compute( src, haystack.keypoints, haystack.descriptor );

	double match,
			best 	= 1.0,
			second = 1.0;
	int		tagId 	= -1;

	// Determine how well each tag matches the haystack, and find the best match.
	for (uint i = 0; i < tags.size(); i++)
	{
		match = matchTag(tags[i], haystack);
		if (match < best)
		{
			second = best;
			best   = match;
			tagId  = i;
		}
	}

	// If the best matching tag is insufficiently close, ignore it.
	if ((best / second) > 0.95f || second == 1.0f)
	{
		ROS_INFO("Best match / second best: %f. Insufficient for recognition.", (best / second));
		return;
	}
	int color = determineColor(src);

	char tagname[50];
	char* colorName;
	if (color == -1) colorName = "Unknown";
	if (color ==  1) colorName = "black";
	if (color ==  2) colorName = "red";
	if (color ==  3) colorName = "green";
	if (color ==  4) colorName = "blue";

	char* objectName;
	if (tagId == -1) objectName = "Unknown";
	if (tagId ==  0) objectName = "apple";
	if (tagId ==  1) objectName = "cup";
	if (tagId ==  2) objectName = "bear";
	if (tagId ==  3) objectName = "glasses";
	if (tagId ==  4) objectName = "dryer";
	if (tagId ==  5) objectName = "laptop";
	if (tagId ==  6) objectName = "glas";
	if (tagId ==  7) objectName = "hammer";
	if (tagId ==  8) objectName = "banana";
	if (tagId ==  9) objectName = "book";
	if (tagId == 10) objectName = "scissors";
	if (tagId == 11) objectName = "camera";

	sprintf(tagname, "snapshots/tags/tag_%d_%s_%s.jpg", imageIndex++, colorName, objectName);
	imwrite(tagname, src);
	ROS_INFO("Tag %d: a %s %s. Certain: %f (%f)", imageIndex - 1, colorName, objectName, best, (best / second));

	tagMessage.dist  = 0.1;
	tagMessage.tagId = tagId;
	tagMessage.colorId = color;
	tag_publisher.publish(tagMessage);
}
